#!/usr/bin/python
import RPi.GPIO as GPIO
import threading
import time


def init_monitor():
    stp = 13
    reverse = 26

    GPIO.setmode(GPIO.BCM)
    GPIO.setup(stp, GPIO.OUT)
    GPIO.setup(19, GPIO.OUT)
    GPIO.setup(reverse, GPIO.OUT)
    GPIO.setup(5, GPIO.IN)
    GPIO.setup(6, GPIO.IN)

    pvelo = GPIO.PWM(19, 20000)
    GPIO.output(reverse, GPIO.HIGH)
    GPIO.output(stp, GPIO.LOW)
    pvelo.start(100)

reverseTag = GPIO.HIGH

def toggleRev():
    global reverseTag
    if reverseTag == GPIO.HIGH:
        reverseTag = GPIO.LOW
    else:
        reverseTag = GPIO.HIGH
    GPIO.output(reverse, reverseTag)

def checkVelocity():
    while True:
        v5 = GPIO.wait_for_edge(5, GPIO.FALLING, timeout = 100)
        t5 = 0
        if v5 is not None:
            t5 = time.time()
            #print("t5 : ", t5)
            v6 = GPIO.wait_for_edge(6, GPIO.FALLING, timeout = 100)
            t6 = 0
            if v6 is not None:
                t6 = time.time()
               # print("v info: ", t5, t6)

def newVelocity(v):
    if v == 0:
        GPIO.output(stp, GPIO.LOW)
    else:
        GPIO.output(stp, GPIO.HIGH)
        if v > 0:
            pvelo.ChangeDutyCycle(v)
        else:
            toggleRev()


def setVelocity():
    while True:
        ddd = float(input("velocity: "))
        if ddd < 0:
            pvelo.ChangeDutyCycle(100) # stop run
            time.sleep(0.1)
            toggleRev()
            pvelo.ChangeDutyCycle(-ddd)
        else:
            pvelo.ChangeDutyCycle(ddd)

if __name__=='__main__':
    initMotor()
    #t1 = threading.Thread(target = checkVelocity)
    #t1.start()
    try:
        while True:
            newVelocity(0)
            newVelocity(-90)
            time.sleep(0.05)
    finally:
        #t1.stop()
        GPIO.cleanup()

